#include "btl_config.h"
#include "fp_led.h"

#define led1_init()                       \
    do {                                  \
        gpio_set_die(LED1_PORT, 1);       \
        gpio_set_direction(LED1_PORT, 0); \
        gpio_write(LED1_PORT, LED_OFF);   \
    } while (0)

#define led2_init()                       \
    do {                                  \
        gpio_set_die(LED2_PORT, 1);       \
        gpio_set_direction(LED2_PORT, 0); \
        gpio_write(LED2_PORT, LED_OFF);   \
    } while (0)

#define led3_init()                       \
    do {                                  \
        gpio_set_die(LED3_PORT, 1);       \
        gpio_set_direction(LED3_PORT, 0); \
        gpio_write(LED3_PORT, LED_OFF);   \
    } while (0)

// #define led1_h() gpio_write(LED1_PORT, 1)
// #define led1_l() gpio_write(LED1_PORT, 0)

// #define led2_h() gpio_write(LED2_PORT, 1)
// #define led2_l() gpio_write(LED2_PORT, 0)
#define LED_LIGHT1(on) gpio_write(LED1_PORT, on)
#define LED_LIGHT2(on) gpio_write(LED2_PORT, on)
#define LED_LIGHT3(on) gpio_write(LED3_PORT, on)

void fp_led_init(void)
{
    led1_init();
    led2_init();
    led3_init();
}

// void fp_led_ctrl(int led, int level)
// {
//     // bf_print_d("led%d %d\r\n", led, level);

//     switch (led) {
//         case FP_LED1: /* R */
//             LED_LIGHT1(level);
//             break;
//         case FP_LED2: /* B */
//             LED_LIGHT2(level); 
//             break;
//         case FP_LED3: /* G */
//             LED_LIGHT3(level);
//             break;
//         default:
//             break;
//     }
// }

/*
  R:  (白光：R+G+B) + 粉红 -> 红色
  G:  (黄光：R+G+0  + 粉红 -> 橙色
  B： (蓝光：0+0+B) + 粉红 -> 紫色
*/
void fp_led_ctrl(int led, int level)
{
    // bf_print_d("led%d %d\r\n", led, level);

    switch (led) {
        case FP_LED1: /* R */
            LED_LIGHT1(level);
            break;
        case FP_LED2: /* B */
            LED_LIGHT2(level); 
            break;
        case FP_LED3: /* G */
            LED_LIGHT3(level);
            break;
        default:
            break;
    }
}


#define moto1_init()                       \
    do {                                  \
        gpio_set_die(MOTO1_PORT, 1);       \
        gpio_set_direction(MOTO1_PORT, 0); \
        gpio_write(MOTO1_PORT, 0);         \
    } while (0)

#define moto2_init()                       \
    do {                                  \
        gpio_set_die(MOTO2_PORT, 1);       \
        gpio_set_direction(MOTO2_PORT, 0); \
        gpio_write(MOTO2_PORT, 0);         \
    } while (0)

#define moto1_h() gpio_write(MOTO1_PORT, 1)
#define moto1_l() gpio_write(MOTO1_PORT, 0)

#define moto2_h() gpio_write(MOTO2_PORT, 1)
#define moto2_l() gpio_write(MOTO2_PORT, 0)

void fp_moto_init(void)
{
    moto1_init();
    moto2_init();
}

void fp_moto(int mode)
{
    switch (mode) {
        case MOTO_STOP:
            moto1_l();
            moto2_l();
            break;
        case MOTO_FORWARD:
            moto1_h();
            moto2_l();
            break;
        case MOTO_REVERSE:
            moto1_l();
            moto2_h();
            break;
        case MOTO_BRAKE:
            moto1_h();
            moto2_h();
            break;
        default:
            break;
    }
}
